#ifndef SAMPLER_H
#define SAMPLER_H

#include "basic_typedef.h"
#include "CollisionDetector.h"

class Sampler {
public:
  ////////////////////////
  // CTORS
  ////////////////////////
  /*  Parameters:
      radius  - radius of robot
      Room  - describes the boundary of the scenario
      CollisionDetector  - Collision detection black box
  */

  Sampler(const Room& room, CollisionDetector *col, double seed = -1) : m_col(col) {  
    Point_2 tl = room.first;
    Point_2 br = room.second;
    xmin = CGAL::to_double(room.first.x());
    xmax = CGAL::to_double(room.second.x());
    ymax = CGAL::to_double(room.second.y());
    ymin = CGAL::to_double(room.first.y());
    lenx = xmax - xmin;
    leny = ymax - ymin;

    if (seed == -1) seed = time(NULL);
    srand(seed);
  }

  ~Sampler(void) { }

  ////////////////////////
  // Queries
  ////////////////////////
  
  /* Returns a point that represents a valid (free) configuration of the robot.
   */
  Point_d generate_sample() const
  {
    {
      while(true) {
        // sample a point in the configuration space
        Point_d p = generate_sample_no_obstacles();

        // validate sample using a collision detector
        if (m_col->valid_conf(p)) return p;
      }
    }
  }

private:
  /* Samples a point in the configuration space of the robot defined by the
   * bounding box of the scenario. This point can be valid or invalid.
   */
  Point_d generate_sample_no_obstacles()  const {
    vector<double> v(4);
    v[0] = xmin + lenx * ((double)rand())/((double)RAND_MAX);
    v[2] = xmin + lenx * ((double)rand())/((double)RAND_MAX);
    v[1] = ymin + leny * ((double)rand())/((double)RAND_MAX);
    v[3] = ymin + leny * ((double)rand())/((double)RAND_MAX);

    return Point_d(v.size(), v.begin(), v.end());
  }

  ////////////////////////
  // Data Members
  ////////////////////////
    
  CollisionDetector* m_col;     // Collision detector

  // room variables
  double xmin;
  double xmax;
  double ymin;
  double ymax;
  double lenx;
  double leny;
};
#endif
